#include "IGHMaster.h"
#include <iostream>
#include <cstring>

IGHMaster::IGHMaster()
    : master(nullptr), domain(nullptr), domain_pd(nullptr), slave_config(nullptr), counter(0) {
    // 初始化状态结构体
    memset(&master_state, 0, sizeof(ec_master_state_t));
    memset(&domain_state, 0, sizeof(ec_domain_state_t));
    memset(&slave_config_state, 0, sizeof(ec_slave_config_state_t));
}

IGHMaster::~IGHMaster() {
    shutdown();
}

bool IGHMaster::init(const std::string& ifname) {
    // IGH不需要网卡名称，直接请求主站
    master = ecrt_request_master(0);
    if (!master) {
        std::cerr << "Failed to request EtherCAT master" << std::endl;
        return false;
    }
    
    // 创建域
    domain = ecrt_master_create_domain(master);
    if (!domain) {
        std::cerr << "Failed to create domain" << std::endl;
        return false;
    }
    
    std::cout << "IGH EtherCAT master initialized successfully" << std::endl;
    return true;
}

bool IGHMaster::configureSlaves() {
    // 这里应该根据实际需求配置从站
    // 示例：配置一个从站
    slave_config = ecrt_master_slave_config(master, 0, 0, 0x00000000, 0x00000000);
    if (!slave_config) {
        std::cerr << "Failed to configure slave" << std::endl;
        return false;
    }
    
    // 注册PDO条目
    // 这里需要根据实际从站配置注册PDO条目
    
    std::cout << "Slaves configured successfully" << std::endl;
    return true;
}

bool IGHMaster::activate() {
    std::cout << "Activating master..." << std::endl;
    if (ecrt_master_activate(master)) {
        std::cerr << "Failed to activate master" << std::endl;
        return false;
    }
    
    // 获取进程数据
    if (!(domain_pd = ecrt_domain_data(domain))) {
        std::cerr << "Failed to get process data" << std::endl;
        return false;
    }
    
    std::cout << "Master activated successfully" << std::endl;
    return true;
}

bool IGHMaster::receiveProcessData() {
    ecrt_master_receive(master);
    ecrt_domain_process(domain);
    return true;
}

bool IGHMaster::sendProcessData() {
    ecrt_domain_queue(domain);
    ecrt_master_send(master);
    return true;
}

bool IGHMaster::checkMasterState() {
    ec_master_state_t ms;
    ecrt_master_state(master, &ms);
    
    if (ms.slaves_responding != master_state.slaves_responding) {
        std::cout << ms.slaves_responding << " slave(s)." << std::endl;
    }
    if (ms.al_states != master_state.al_states) {
        std::cout << "AL states: 0x" << std::hex << ms.al_states << std::dec << std::endl;
    }
    if (ms.link_up != master_state.link_up) {
        std::cout << "Link is " << (ms.link_up ? "up" : "down") << std::endl;
    }
    
    master_state = ms;
    return true;
}

bool IGHMaster::checkDomainState() {
    ec_domain_state_t ds;
    ecrt_domain_state(domain, &ds);
    
    if (ds.working_counter != domain_state.working_counter) {
        std::cout << "Domain: WC " << ds.working_counter << std::endl;
    }
    if (ds.wc_state != domain_state.wc_state) {
        std::cout << "Domain: State " << ds.wc_state << std::endl;
    }
    
    domain_state = ds;
    return true;
}

bool IGHMaster::checkSlaveConfigState() {
    ec_slave_config_state_t s;
    ecrt_slave_config_state(slave_config, &s);
    
    if (s.al_state != slave_config_state.al_state) {
        std::cout << "Slave: State 0x" << std::hex << s.al_state << std::dec << std::endl;
    }
    if (s.online != slave_config_state.online) {
        std::cout << "Slave: " << (s.online ? "online" : "offline") << std::endl;
    }
    if (s.operational != slave_config_state.operational) {
        std::cout << "Slave: " << (s.operational ? "" : "Not ") << "operational" << std::endl;
    }
    
    slave_config_state = s;
    return true;
}

void IGHMaster::shutdown() {
    if (master) {
        // 释放主站资源
        ecrt_release_master(master);
        master = nullptr;
        domain = nullptr;
        domain_pd = nullptr;
        slave_config = nullptr;
        std::cout << "IGH EtherCAT master shut down" << std::endl;
    }
}

void IGHMaster::cyclicTask() {
    // 接收过程数据
    receiveProcessData();
    
    // 检查过程数据状态
    checkDomainState();
    
    if (counter) {
        counter--;
    } else { // 每秒执行一次
        counter = 1000; // 假设周期为1ms，则1000次为1秒
        
        // 检查主站状态
        checkMasterState();
        
        // 检查从站配置状态
        checkSlaveConfigState();
    }
    
    // 这里可以添加读写过程数据的代码
    // 例如：EC_WRITE_U8(domain_pd + offset, value);
    
    // 发送过程数据
    sendProcessData();
}